/**
    Copyright (c) 2010 yakiimo02
    Distributed under the New BSD License.
    See included license.txt or http://www.yakiimo3d.com/NewBSDLicense.txt
**/

#pragma once

#include "optimized/SmallPtGeometryOPT.h"
#include "optimized/SmallPtDefines.h"
#include "optimized/SmallPtMath.h"
#include "optimized/SmallPtStructs.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>  

namespace OPT
{

/**
    @brief   Sphere class for SOA processing.
             Represents 4 spheres.
    @author  yakiimo02
    @date    2010/08
*/
struct SphereSOA 
{
public:
    __m128 posX;            // position x
    __m128 posY;            // position y
    __m128 posZ;            // position z
    __m128 radiusSq;        // radius squared

public:
    /**
        Convert 4 AOS spheres to a 4 SOA spheres.
    */
    inline void Init( const Sphere* spheres )
    {
        posX = VectorSet( spheres[ 0 ].position.x, spheres[ 1 ].position.x, spheres[ 2 ].position.x, spheres[ 3 ].position.x );
        posY = VectorSet( spheres[ 0 ].position.y, spheres[ 1 ].position.y, spheres[ 2 ].position.y, spheres[ 3 ].position.y );
        posZ = VectorSet( spheres[ 0 ].position.z, spheres[ 1 ].position.z, spheres[ 2 ].position.z, spheres[ 3 ].position.z );
        radiusSq = VectorSet( spheres[ 0 ].radius*spheres[ 0 ].radius, spheres[ 1 ].radius*spheres[ 1 ].radius, 
            spheres[ 2 ].radius*spheres[ 2 ].radius, spheres[ 3 ].radius*spheres[ 3 ].radius );
    }
};

// global SOA spheres
extern __declspec(selectany) SphereSOA g_sphereSOAs[ 3 ];

/**
    Initialize the global SOA spheres.
*/
inline void InitSphereSOAs()
{
    g_sphereSOAs[ 0 ].Init( &spheresOPT[ 0 ] );
    g_sphereSOAs[ 1 ].Init( &spheresOPT[ 4 ] );

    Sphere lastSphere[ 4 ];
    lastSphere[ 0 ] = spheresOPT[ 8 ];
    lastSphere[ 1 ] = spheresOPT[ 8 ];
    lastSphere[ 2 ] = spheresOPT[ 8 ];
    lastSphere[ 3 ] = spheresOPT[ 8 ];

    g_sphereSOAs[ 2 ].Init( &lastSphere[ 0 ] );
}

/**
    @brief   Ray for intersection with SOA spheres. 
             Represents a single ray.
    @author  yakiimo02
    @date    2010/08
*/
struct RaySOA
{
    __m128 posX;        
    __m128 posY;           
    __m128 posZ;
    __m128 dirX;
    __m128 dirY;
    __m128 dirZ;

    /**
        Convert ray to format more suitable for SOA processing.
    */
    inline void Init( const Ray& ray )
    {
        posX = VectorSet( ray.o.x );
        posY = VectorSet( ray.o.y );
        posZ = VectorSet( ray.o.z );
        dirX = VectorSet( ray.d.x );
        dirY = VectorSet( ray.d.y );
        dirZ = VectorSet( ray.d.z );
    }
};

/**
    SIMD SOA ray-sphere intersection.
    @param [in] ray.
    @param [in] Index to global SOA sphere array.
*/
__forceinline __m128 IntersectSOA( const RaySOA& ray, int nIndex )
{
    SphereSOA sphere =  g_sphereSOAs[ nIndex ];

    // Vec op = position-(r.o); 
    __m128 opX = sphere.posX - ray.posX;
    __m128 opY = sphere.posY - ray.posY;
    __m128 opZ = sphere.posZ - ray.posZ;

    // SPT_FLOAT_OPT eps=1e-3;
    const __m128 eps = VectorSet( 1e-3f );

    // SPT_FLOAT_OPT b=op.dot(r.d);
    const __m128 b = ( opX * ray.dirX ) + ( opY * ray.dirY ) + ( opZ * ray.dirZ );

    // SPT_FLOAT_OPT det=b*b-op.dot(op)+radius*radius;
    const __m128 opDot = opX*opX+opY*opY+opZ*opZ;
    __m128 det = b*b - opDot + sphere.radiusSq;

    // mask of whether determinant was less than zero
    __m128 detLtZeroMask = _mm_cmplt_ps( det, _mm_setzero_ps() );

    // early exit if all determinants are less than zero
    if( ( (_mm_movemask_ps(detLtZeroMask)&15) == 15 ) != 0 ) 
    {
        return _mm_setzero_ps();
    }

    // det=sqrt(det);
    det = _mm_sqrt_ps( det );

    __m128 distance = _mm_setzero_ps();
    __m128 t0;
    __m128 t1;

    // Check both whether b-det, b+det > eps.
    t0 = b - det;
    t1 = b + det;

    __m128 gtEpsMask0 = _mm_cmpgt_ps( t0, eps );
    __m128 gtEpsMask1 = _mm_cmpgt_ps( t1, eps );
    
    distance = Select( distance, t1, gtEpsMask1 );
    distance = Select( distance, t0, gtEpsMask0 );

    distance = Select( distance, _mm_setzero_ps(), detLtZeroMask );

    return distance;
}

}